/*=====================================================================*
 *                   Copyright (C) 2011 Paul Mineiro                   *
 * All rights reserved.                                                *
 *                                                                     *
 * Redistribution and use in source and binary forms, with             *
 * or without modification, are permitted provided that the            *
 * following conditions are met:                                       *
 *                                                                     *
 *     * Redistributions of source code must retain the                *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer.                                       *
 *                                                                     *
 *     * Redistributions in binary form must reproduce the             *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer in the documentation and/or            *
 *     other materials provided with the distribution.                 *
 *                                                                     *
 *     * Neither the name of Paul Mineiro nor the names                *
 *     of other contributors may be used to endorse or promote         *
 *     products derived from this software without specific            *
 *     prior written permission.                                       *
 *                                                                     *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND              *
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,         *
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES               *
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE             *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER               *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                 *
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES            *
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE           *
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR                *
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF          *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           *
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY              *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE             *
 * POSSIBILITY OF SUCH DAMAGE.                                         *
 *                                                                     *
 * Contact: Paul Mineiro <paul@mineiro.com>                            *
 *=====================================================================*/

#ifndef __BOGACKI_SHAMPINE_HH__
#define __BOGACKI_SHAMPINE_HH__

#include <algorithm>
#include <cmath>
#include <utility>

namespace playerpiano
{
  // Solve { y'[x] == f[x, y[x]], y[x0] == y0 } return y[x1]

  template<typename F,
           typename T>
  std::pair<bool, T>
  bogacki_shampine_2_3 (const F&     f,
                        float        x0,
                        T            y0,
                        float        x1,
                        float        tol,
                        float        hmin,
                        float        hmax,
                        unsigned int maxevals)
    {
      std::pair<bool, T> rv (false, y0);

      // Tableau from
      // http://en.wikipedia.org/wiki/Bogacki-Shampine_method

      float c2[] = { 1.0f / 2.0f, 1.0f / 2.0f };
      float c3[] = { 3.0f / 4.0f, 0.0f, 3.0f / 4.0f };
      float c4[] = { 1.0f, 2.0f / 9.0f, 1.0f / 3.0f, 4.0f / 9.0f };

      float b1[] = { 2.0f / 9.0f, 1.0f / 3.0f, 4.0f / 9.0f, 0.0f };
      float b2[] = { 7.0f / 24.0f, 1.0f / 4.0f, 1.0f / 3.0f, 1.0f / 8.0f };

      float x = x0;
      T& y (rv.second);
      T f1 (f (x, y));
      float h = std::min (hmax, x1 - x);

      for (unsigned int n = 1; n < maxevals && ! rv.first; n += 3)
        {
          T f2 (f (x + c2[0] * h, y + h * (c2[1] * f1)));
          T f3 (f (x + c3[0] * h, y + h * (/* c3[1] * f1 + */ c3[2] * f2)));
          T f4 (f (x + c4[0] * h, y + h * (c4[1] * f1 + c4[2] * f2 + c4[3] * f3)));

          float errorsq = ((b1[0] - b2[0]) * f1 +
                           (b1[1] - b2[1]) * f2 +
                           (b1[2] - b2[2]) * f3 +
                           (/* b1[3] */ - b2[3]) * f4) * 
                          ((b1[0] - b2[0]) * f1 +
                           (b1[1] - b2[1]) * f2 +
                           (b1[2] - b2[2]) * f3 +
                           (/* b1[3] */ - b2[3]) * f4);

          if (errorsq < tol * tol)
            {
              x = x + h;
              y += h * (c4[1] * f1 + 
                        c4[2] * f2 + 
                        c4[3] * f3);
              f1 = f4;
            }

          if (x - x1 >= -1e-4 * fabs (x1))
//          if (x >= x1)
            {
              rv.first = true;
              break;
            }

          float delta = 0.84f * powf (tol * tol / errorsq, 1.0f / 6.0f);

          h = std::min (hmax, h * std::min (4.0f, std::max (delta, 0.1f)));

          if (h < hmin) // fail (!)
            {
              break;
            }

          h = std::min (h, x1 - x);
        }

      return rv;
    }
}

#endif // __BOGACKI_SHAMPINE_HH__
